/**
*/


#include <OGF/quick_start/common/refinement.h>
#include <OGF/quick_start/common/select_util.h>
#include <OGF/basic/os/stopwatch.h>
#include <OGF/cells/map/map_cell_heap.h>

#include <set>
#include <stack>
#include <queue>
#include <limits>
#include <list>

#include <OGF/math/linear_algebra/vector.h>

using namespace std;

namespace OGF 
{
	//	The skeleton vertices get moved to the reweighted position/average of its respective
	//	non-skeleton vertices.
	void Refinement::Refine(Surface* oldSurface, Line* skeletonSurface) {
		calculateAvgDisplacements(oldSurface, skeletonSurface);
		repositionSkeletonNodes(oldSurface, skeletonSurface);
	}

	//next bordering vertex must not be marked, must be on border,
	//must share non-border vertex
	Map::Vertex* getNextBorderVert(Surface* s, Map::Vertex* v) {
		MapVertexAttribute<bool> onBorder(s, "REFINE_BORDER_VERTEX");
		MapVertexAttribute<bool> ringMarked(s, "REFINE_RING_MARKED");
		
		Map::Halfedge* he = nil;
		FOR_EACH_ADJACENT_OUTEDGE(v, he) {
			bool vertBorder, vertMarked, nextVertBorder, nextVertMarked;
			vertBorder = onBorder[he->vertex()];
			vertMarked = ringMarked[he->vertex()];
			nextVertBorder = onBorder[he->next()->vertex()];
			nextVertMarked = ringMarked[he->next()->vertex()];

			//If vertex is on border, not marked and the next vertex is not on the border
			if (vertBorder && !vertMarked && !nextVertBorder) 
				return he->vertex();
			else if (nextVertBorder && !nextVertMarked && !vertBorder)
				return he->next()->vertex();
		}
		return nil;
	}

	void Refinement::calculateVertexAvgDisplacement(Surface* oldSurface, Line* skeletonSurface, Graph::Vertex* in_v) {
		//input
		GraphVertexAttribute<VertexSet> skeletonAssociatedVerts(skeletonSurface, "ASSOCIATED_VERTS" );
		MapVertexAttribute<Point3d> originalVertexPosition(oldSurface, "ORGINIAL_POSITION");
		MapVertexAttribute<Point3d> contractedVertexPosition(oldSurface, "CONTRACTED_POSITION");

		//output
		GraphVertexAttribute<Vector3d> displacements(skeletonSurface, "DISPLACEMENTS");

		//locals
		MapVertexAttribute<bool> onBorder(oldSurface, "REFINE_BORDER_VERTEX");
		MapVertexAttribute<bool> ringMarked(oldSurface, "REFINE_RING_MARKED");

		VertexSet oldVertexSet(skeletonAssociatedVerts[in_v]);

		//Step 1: Label vertices in the set as border or not
		//	Results stored in 
		//			MapVertexAttribute<bool> onBorder 
		VertexSet::iterator v_it = oldVertexSet.begin();
		Map::Vertex *v = nil;

		/*
		for ( ; v_it != oldVertexSet.end(); ++v_it) {
			Map::Vertex* currVert = (*v_it);
			onBorder[currVert] = false;
			FOR_EACH_ADJACENT_VERTEX(currVert, v) {
				//Adjacent vertex not in set -> vertex is on border
				if (oldVertexSet.find(v) == oldVertexSet.end()) 
					onBorder[currVert] = true;
			}
			// for later, we set ringmarked to be false
			ringMarked[currVert] = false;
		}*/

		
		//Step 2: Construct rings
		//		each ring is a set of vertices
		set<VertexSet> ringSet;
		v_it = oldVertexSet.begin();
		/*
		for ( ; v_it != oldVertexSet.end(); ++v_it) {
			Map::Vertex* currVert = (*v_it);
			
			//If not marked, then start at that vert and go along ring 
			if (!ringMarked[currVert]) {
				VertexSet currentRing;
				//printf("NEW RING\n");

				//Go around ring
				while (currVert != nil) {
					//printf("  ADD TO RING %d CurrentVert %d\n", &currentRing, currVert);
					ringMarked[currVert] = true;
					currentRing.insert(currVert);

					currVert = getNextBorderVert(oldSurface, currVert);	
				}
				
				// add the ring to the set		
				if (currentRing.size() != 0)
					ringSet.insert(currentRing);
			} 	
		}



		*/
		//printf("NumRings %d\n", ringSet.size());

		
		//Step 3: Calculate the displacement vector for each ring
		list<Vector3d> ringDisplacements;
		set<VertexSet>::iterator ring_it = ringSet.begin();

		//Case 1, general case -> 2 or more borders so weight based on borders
		/*
		if (ringSet.size() > 1) {
			for ( ; ring_it != ringSet.end(); ++ring_it) {		//for each (ring_it in ringSet)
				VertexSet ringVertexSet = (*ring_it);
				VertexSet::iterator rv_it = ringVertexSet.begin();

				double totalLength = 0.0;
				Vector3d totalDisplacement;

				for ( ; rv_it != ringVertexSet.end(); ++rv_it) {	//for each (rv_it in ringVertexSet) aka all the vertices in a ring
					Map::Vertex* currVert = (*rv_it);

					//add lengths - should be two per vert as it is a border
					double lengths = 0.0;
					Vector3d displacement;

					int numBorderVerts = 0;
					FOR_EACH_ADJACENT_VERTEX(currVert, v) {
						if (onBorder[v]) {
							lengths += (v->point() - currVert->point()).norm();
							numBorderVerts++;
						}
					}
			
					displacement = lengths*((currVert->point()) - originalVertexPosition[currVert]);

					totalDisplacement = totalDisplacement + displacement;
					totalLength += lengths;
				}

				totalDisplacement = totalDisplacement / totalLength;
				ringDisplacements.push_back(totalDisplacement);
			}
		} // end Case 1
		else if (ringSet.size() == 1) { //case two, one border -> end of skeleton, use the whole region and
										//compute average displacement
										*/
		v_it = oldVertexSet.begin();

		double totalLength = 0.0;
		Vector3d totalDisplacement;

		for ( ; v_it != oldVertexSet.end(); ++v_it) {
			Map::Vertex* currVert = (*v_it);

			Vector3d displacement = (contractedVertexPosition[currVert] - originalVertexPosition[currVert]);
			totalDisplacement = totalDisplacement + displacement;				
		}
		totalDisplacement = totalDisplacement / ((double)oldVertexSet.size());

		ringDisplacements.push_back(totalDisplacement);
		//}

		
		//Step 5: Get the average displacement over all rings for case 1 OR region for case 2
		list<Vector3d>::iterator d_it = ringDisplacements.begin();
		Vector3d avgDisplacement;
		for ( ; d_it != ringDisplacements.end(); ++d_it) {		//for each (d_it in ringDisplacements)
			avgDisplacement = avgDisplacement + (*d_it);	
		}
		avgDisplacement = avgDisplacement / ((double)ringDisplacements.size());

		//Step 6: Now that we have the displacements, store them per vertex
		displacements[in_v] = avgDisplacement;	

		printf("displacement: %f %f %f\n", avgDisplacement.x(), avgDisplacement.y(), avgDisplacement.z());
	}

	//Calculate the average displacements per skeleton vertex
	void Refinement::calculateAvgDisplacements(Surface* oldSurface, Line* skeletonSurface) {
		FOR_EACH_VERTEX(Graph, skeletonSurface, it) {
			calculateVertexAvgDisplacement(oldSurface, skeletonSurface, it);
		}
	}

	//Use the calculated average displacements to reposition the point
	void Refinement::repositionSkeletonNode(Surface* oldSurface, Line* skeletonSurface, Graph::Vertex* v) {
		GraphVertexAttribute<Vector3d> displacements(skeletonSurface, "DISPLACEMENTS");

		Point3d newPosition = v->point() - displacements[v];
		v->set_point(newPosition);
	}

	//Reposition all the nodes in the skeleton surface
	void Refinement::repositionSkeletonNodes(Surface* oldSurface, Line* skeletonSurface) {
		FOR_EACH_VERTEX(Graph, skeletonSurface, it) {
			repositionSkeletonNode(oldSurface, skeletonSurface, it);
		}
	}

} //namespace
